#include "iirfilter.h"
#include <stdio.h>
#include <cmath>
#include <iostream>
#include <cstring>
#include <stdio.h>

/**
 * Init the filter operation
 */
iirfilter::iirfilter() { }

/**
 * Destructor
 */
iirfilter::~iirfilter(){ }


/**
 * Init the filter operation
 */
void iirfilter::init(tIirMainStructure *tIirMainStruct_)
{   
    for (int i=0; i < MAX_IIR_ORDER; i++)
    {
        tIirMainStruct_->iir_w_n[i] = 0.0;
    }

    for (int i=0; i < 2*MAX_IIR_ORDER; i++)
    {
        tIirMainStruct_->iir_coefs[i] = 0.0;
    }

    tIirMainStruct_->iir_out_tmp = 0.0;

    /* keep the initial filter order on main structure */
    tIirMainStruct_->iir_order = INIT_IIR_ORDER;
    tIirMainStruct_->iir_fc1   = INIT_FREC_C0;
    tIirMainStruct_->iir_fc2   = INIT_FREC_C1;


    tIirMainStruct_->iirType=IIR_COEFF_LOW;

    setArguments(tIirMainStruct_);
}

void  iirfilter::setArguments(tIirMainStructure *tIirMainStruct_)
{
    /* Declaration of local variables */
    float arg_sk;
    float real_sk;
    float wc_lp,wc_hp;
    float cuad_wc,cuad_wp;
    float T;
    float cuad_T;
    float cuad_Tdiv4;
    int N_2;
    int N;
    int coeff_N;
    float lambda,xi,eta;
    eIirType type;

    type = tIirMainStruct_->iirType;

    T  = 1 / (SAMPLING_FREQ);
    cuad_T = (T * T);
    cuad_Tdiv4 = cuad_T/4;

    /* calculation of wc , the cuad_wc is previously calculated to avoid
     * multiplications on loop that will increase the execution time unnecesaarry
     * on an embedded system multiplcation time is one of the operation that take
     * more instructions , an optimization like this will save MCPS
     */
    wc_lp = (2 * M_PI * tIirMainStruct_->iir_fc1);
    wc_hp = (2 * M_PI * tIirMainStruct_->iir_fc2);

    cuad_wc = wc_lp * wc_lp;
    cuad_wp = wc_hp * wc_hp;
    coeff_N = 1;
    N = tIirMainStruct_->iir_order;
    N_2 = N / 2;


    switch (type)
    {
    case IIR_COEFF_LOW:

        tIirMainStruct_->iir_coefs[0] = 1;

        for(int k = 0; k < N_2; k++)
        {
            arg_sk  = (M_PI/2) + (2*k + 1) * M_PI/(2 * N);
            real_sk = wc_lp * cos(arg_sk);

            lambda  = 1 - (T * real_sk) + (cuad_Tdiv4 * cuad_wc);
            xi      = 1 + (T * real_sk) + (cuad_Tdiv4 * cuad_wc);

            tIirMainStruct_->iir_coefs[0]         = (tIirMainStruct_->iir_coefs[0]) * ( (cuad_T * cuad_wc ) / (4 * lambda) );
            tIirMainStruct_->iir_coefs[coeff_N]   = tIirMainStruct_->iir_out_tmp = - 2 * (1-(cuad_Tdiv4)*cuad_wc)/lambda;
            tIirMainStruct_->iir_coefs[coeff_N+1] = tIirMainStruct_->iir_out_tmp = xi / lambda;
            tIirMainStruct_->iir_coefs[coeff_N+2] = tIirMainStruct_->iir_out_tmp = 2;
            tIirMainStruct_->iir_coefs[coeff_N+3] = tIirMainStruct_->iir_out_tmp = 1;
            coeff_N += 4;
        }
        break;

    case IIR_COEFF_HIGH:

        tIirMainStruct_->iir_coefs[0] = 1;


        for(int k = 0; k < N_2; k++)
        {
            arg_sk  = (M_PI/2) + (2*k + 1) * M_PI/(2 * N);
            real_sk = wc_lp * cos(arg_sk);

            lambda  = cuad_wp*cuad_T*wc_lp - 4*(T * real_sk* wc_hp) + (4 * wc_lp);
            eta     = 2 * (4 * wc_lp - cuad_wp*cuad_T*wc_lp);
            xi      = cuad_wp*cuad_T*wc_lp + 4*(T * real_sk* wc_hp) + (4 * wc_lp);


            tIirMainStruct_->iir_coefs[0]         = (tIirMainStruct_->iir_coefs[0]) * (4)*( (wc_lp) / (lambda) );
            tIirMainStruct_->iir_coefs[coeff_N]   = tIirMainStruct_->iir_out_tmp = - eta / lambda;
            tIirMainStruct_->iir_coefs[coeff_N+1] = tIirMainStruct_->iir_out_tmp = xi / lambda;
            tIirMainStruct_->iir_coefs[coeff_N+2] = tIirMainStruct_->iir_out_tmp = -2;
            tIirMainStruct_->iir_coefs[coeff_N+3] = tIirMainStruct_->iir_out_tmp = 1;
            coeff_N += 4;
        }
        break;

    case IIR_COEFF_BAND:
    case IIR_COEFF_STOP:
    break;
    }

}

/**
 * IIRFILTER
 */
void iirfilter::filter(
    int blockSize,
    float* in,
    float* out,
    tIirMainStructure *tIirMainStruct_)
{   
    /* Declaration of local variables */
    int N;
    int N_2;

    /* initialization of local variables */
    N   = tIirMainStruct_->iir_order;
    N_2 = N/2;

    for (int n = 0 ; n < blockSize; n++)
    {
        out[n] = in[n] * (tIirMainStruct_->iir_coefs[0]);

        for(int k = 0; k < N_2 ; k++)
        {
            tIirMainStruct_->iir_w_tmp = out[n] -
                    ( tIirMainStruct_->iir_w_n[k] * tIirMainStruct_->iir_coefs[4*k + 1] ) -
                    ( tIirMainStruct_->iir_w_n[k + N_2] * tIirMainStruct_->iir_coefs[4*k + 2]);

            out[n] = tIirMainStruct_->iir_w_tmp +
                    (tIirMainStruct_->iir_w_n[k] * tIirMainStruct_->iir_coefs[4*k + 3]) +
                    (tIirMainStruct_->iir_w_n[k + N_2] * tIirMainStruct_->iir_coefs[4*k+4]);

            tIirMainStruct_->iir_w_n[k+N_2]=tIirMainStruct_->iir_w_n[k];
            tIirMainStruct_->iir_w_n[k]=tIirMainStruct_->iir_w_tmp;
            tIirMainStruct_->iir_out_tmp = out[n];
        }
    }
}

/*
 * Reset
 */
void iirfilter::reset(tIirMainStructure *tIirMainStruct_)
{
    init(tIirMainStruct_);
}


void iirfilter::GetIirInfo (tIirMainStructure* tIirMainStruct_)
{
    FILE * pFile;
    int n;
    int order = tIirMainStruct_->iir_order;

    pFile = fopen ("iir_filter_info.m","w");


    fprintf (pFile,"%% matlab file just use it \n");
    fprintf (pFile,"SAMPLING_FREQ = %f;\n",SAMPLING_FREQ);
    fprintf (pFile,"ORDER = %i;\n",order);

    switch (tIirMainStruct_->iirType)
    {
    case IIR_COEFF_LOW:
        fprintf (pFile,"%% Type: IIR_COEFF_LOW\n");
        fprintf (pFile,"CUT_FREQ = %f;\n",tIirMainStruct_->iir_fc1);
        break;
    case IIR_COEFF_HIGH:
        fprintf (pFile,"%% Type: IIR_COEFF_HIGH\n");
        fprintf (pFile,"CUT_FREQ : %f;\n",tIirMainStruct_->iir_fc2);
        break;
    case IIR_COEFF_BAND:
        fprintf (pFile,"%% Type: IIR_COEFF_BAND\n");
        break;
    case IIR_COEFF_STOP:
        fprintf (pFile,"%% Type: IIR_COEFF_STOP\n");
        break;
    }

    fprintf (pFile, "COEF = [ ");

    for (n=0 ; n<order ; n++)
    {
        fprintf (pFile, "%f ",tIirMainStruct_->iir_coefs[n]);
    }
    fprintf (pFile, " ]; \n\n");

    fclose (pFile);

}
